#include <ir/ir.h>

#include <sensor/IR.h>
#include <simulation/Distance.h>
#include <string>

IR::IR(int argc, char **argv)
{
	ros::NodeHandle nh("ir");

	nh.getParam("simulation", simulation);
	nh.getParam("sample_rate", sample_rate);

	// Announce the service that serves information about the IR sensors
	sensor_info_server_ = nh.advertiseService("/sensor/ir/info", &IR::sensor_info_callback, this);

	distance_publisher = nh.advertise<sensor::IR>("/sensor/ir", 0);
	adc_subscriber = nh.subscribe("/roboard/adc", 1, &IR::receiveADC, this);

	if (simulation) {
		distance_client = nh.serviceClient<simulation::Distance>("/simulation/world/distance");
		distance_client.waitForExistence();
		distance_client = nh.serviceClient<simulation::Distance>("/simulation/world/distance", true);
	}

	addSensors(&nh);

	ros::Rate loop_rate(sample_rate);

	std::vector<IRSensor *>::iterator it;
	sensor::IR irMessage;
	while (ros::ok()) {
		if (simulation && !distance_client) {
			ROS_ERROR("Error: Distance client lost connection.");
		}

		int sensor_id = 0;
		for (it = sensors_.begin(); it < sensors_.end(); ++it) {
			float distance;

			IRSensor *sensor = *it;
			if (simulation) {
				distance = sensor->simulatedMeasure(&distance_client);
			} else {
				distance = sensor->measure();
			}

			irMessage.dists[sensor_id] = distance;

			sensor_id++;
		}

		distance_publisher.publish(irMessage);

		// Allow callbacks to be called
		ros::spinOnce();

		// Limit loop rate by calling sleep()
		loop_rate.sleep();
	}
}


void IR::receiveADC(const roboard_drivers::adc_val::ConstPtr &msg)
{
	sensors_[0]->setADCValue(msg->val0);
	sensors_[1]->setADCValue(msg->val1);
	sensors_[2]->setADCValue(msg->val2);
	sensors_[3]->setADCValue(msg->val3);
	sensors_[4]->setADCValue(msg->val4);
	sensors_[5]->setADCValue(msg->val5);
	sensors_[6]->setADCValue(msg->val6);
	sensors_[7]->setADCValue(msg->val7);
}

bool IR::sensor_info_callback(sensor::IRInfo::Request &req, sensor::IRInfo::Response &res)
{
	if (req.sensor_id < 0 || req.sensor_id > (int) sensors_.size()) {
		ROS_ERROR("Requesting info about nonexistant sensor id %d", req.sensor_id);
		return false;
	}

	IRSensor *sensor = sensors_[req.sensor_id];

	res.x = sensor->getX();
	res.y = sensor->getY();
	res.z = sensor->getZ();
	res.angle = sensor->getAngle();
	
	return true;
}

IR::~IR()
{
	std::vector<IRSensor *>::iterator it;
	for (it = sensors_.begin(); it < sensors_.end(); ++it) {
		delete *it;
	}
}

void IR::addSensors(ros::NodeHandle *nh)
{
	int num_sensors;
	nh->getParam("num_sensors", num_sensors);

	ROS_INFO("Creating %d sensors with data from parameter server.", num_sensors);

	char string[100];
	for (int i = 0; i < 8; i++) {
		std::string stringType;
		double x, y, z, angle;

		snprintf(string, sizeof(string), "s%d/x", i);
		nh->getParam(string, x);
		snprintf(string, sizeof(string), "s%d/y", i);
		nh->getParam(std::string(string), y);
		snprintf(string, sizeof(string), "s%d/z", i);
		nh->getParam(std::string(string), z);
		snprintf(string, sizeof(string), "s%d/angle", i);
		nh->getParam(std::string(string), angle);
		snprintf(string, sizeof(string), "s%d/type", i);
		nh->getParam(std::string(string), stringType);

		IRSensorType type;
		if (stringType.compare("ir_short")) {
			type = IRSensorTypeShort;
		} else {
			type = IRSensorTypeLong;
		}

		IRSensor *sensor = new IRSensor(i, x, y, z, angle, type);
		sensors_.push_back(sensor);
	}
}

int main (int argc, char **argv)
{
  	ros::init(argc, argv, "ir");
	new IR(argc, argv);
	return 0;
}
